//!Includes

#include "Target.h"

//!Global Functions


namespace algorithms
{

/**
 * class Target
 *
 *
 * Autor:David Bevilaqua
 *
 *
 * DESCRIPTION:
 *
 */

//!Constructors/Destructors

/**
 * Empty Constructor
 */
Target::Target ( )
{

}

/**
 * Empty Destructor
 */
Target::~Target ( )
{
//cvReleaseMat(&m_pPointsRef);
//cvReleaseMat(&m_pPointCurent);
//cvReleaseMat(&H);
}

//!Public Accessor Methods
//

int Target::GetP()
{
return  std::sqrt(std::abs(m_PoseTarget.GetPose().x-m_PoseRobot.GetPose().x)*std::abs(m_PoseTarget.GetPose().x-m_PoseRobot.GetPose().x)+std::abs(m_PoseTarget.GetPose().y-m_PoseRobot.GetPose().y)*std::abs(m_PoseTarget.GetPose().y-m_PoseRobot.GetPose().y));
}

int Target::GetAlfa()
{
return  -m_PoseRobot.GetOrientation()+180*std::atan2(m_PoseTarget.GetPose().y-m_PoseRobot.GetPose().y,m_PoseTarget.GetPose().x-m_PoseRobot.GetPose().x)/3.1415;
}

int Target::GetBeta()
{
return m_PoseTarget.GetOrientation()-180*std::atan2(m_PoseTarget.GetPose().y-m_PoseRobot.GetPose().y,m_PoseTarget.GetPose().x-m_PoseRobot.GetPose().x)/3.1415;
}

void Target::SetPoseRobot(CvPoint p,int th)
{
m_PoseRobot.SetPose(p);
m_PoseRobot.SetOrientation(th);
}

Pose Target::GetPoseRobot( )
{
return m_PoseRobot;
}

void Target::SetPoseTarget(CvPoint p,int th)
{
m_PoseTarget.SetPose(p);
m_PoseTarget.SetOrientation(th);
}

Pose Target::GetPoseTarget( )
{
return m_PoseTarget;
}

int Target::GetOrientation( )
{
	//printf("m_iUpTarget:%d,m_iDownTarget:%d,m_iLeftTarget:%d,m_iRightTarget:%d\n",m_iUpTarget,m_iDownTarget,m_iLeftTarget,m_iRightTarget);
	if((m_iUpTarget)<((double)m_iRightTarget+(double)m_iLeftTarget)/2)
	{
		//printf("thetha: %f  \n",180*std::acos((double)(m_iUpTarget)/(((double)m_iRightTarget+(double)m_iLeftTarget)/2))/3.1415);
		return 180*std::acos((double)(m_iUpTarget)/(((double)m_iRightTarget+(double)m_iLeftTarget)/2))/3.1415;
		//printf("thetha: %f  \n",100*(1-(double)(m_iUpTarget)/((double)((m_iRightTarget+m_iLeftTarget)/2))));
		//return 100*(1-(double)(m_iUpTarget)/(double)((m_iRightTarget+m_iLeftTarget)/2));


	}
	else
	{
		//printf("theta: 0\n");
		return 0;
	}

}

int Target::GetOrientationRef( )
{

	if((m_iUpTargetRef)<((double)m_iRightTargetRef+(double)m_iLeftTargetRef)/2)
	{
		//printf("thethaRef: %f  \n",180*std::acos((double)(m_iUpTargetRef)/(((double)m_iRightTargetRef+(double)m_iLeftTargetRef)/2))/3.1415);
		return 180*std::acos((double)(m_iUpTargetRef)/(((double)m_iRightTargetRef+(double)m_iLeftTargetRef)/2))/3.1415;
		//printf("thethaRef: %f  \n",100*(1-(double)(m_iUpTargetRef)/(double)((m_iRightTargetRef+m_iLeftTargetRef)/2)));
		//return 100*(1-(double)(m_iUpTargetRef)/((double)((m_iRightTargetRef+m_iLeftTargetRef)/2)));


	}
	else
	{
		//printf("thetaRef: 0\n");
		return 0;
	}



}

int  Target::GetDiffOrientation( )
{
//printf("GetDiffOrientation: %d \n",std::abs(GetOrientationRef()-GetOrientation()));
return std::abs(GetOrientationRef()-GetOrientation());
}

int Target::GetEstOrientation()
{
return 100*(double)m_iEstOrientation;
}

int Target::GetEstOrientationRef()
{
return 100*(double)m_iEstOrientationRef;
}

int  Target::GetDiffEstOrientation()
{
//printf("GetDiffEstOrientation: %d \n",GetEstOrientationRef()-GetEstOrientation());
return GetEstOrientationRef()-GetEstOrientation();
}

int Target::GetDiffRightTarget()
{
return m_iRightTargetRef-m_iRightTarget;
}

int Target::GetDiffLeftTarget()
{
return m_iLeftTargetRef-m_iLeftTarget;
}

int Target::GetDiffUpTarget()
{
return m_iUpTargetRef-m_iUpTarget;
}

int Target::GetDiffCenterX()
{
return m_TargetCenterRef.x-m_TargetCenter.x;
}

int Target::GetDiffCenterY()
{
return m_TargetCenterRef.y-m_TargetCenter.y;
}

int Target::GetDiffDiagonal()
{
return (m_iDiagonal1Ref+m_iDiagonal2Ref)/2-(m_iDiagonal1+m_iDiagonal2)/2;
}

int Target::GetDiagonal()
{
return (m_iDiagonal1+m_iDiagonal2)/2;
}

CvPoint Target::GetCenter()
{
return m_TargetCenter;
}
//!Public Methods
//

void Target::Sort()
{
	CvPoint m_TargetPointsTemp[5];
	m_TargetPointsTemp[0]=m_TargetPoints[0];
	m_TargetPointsTemp[1]=m_TargetPoints[1];
	m_TargetPointsTemp[2]=m_TargetPoints[2];
	m_TargetPointsTemp[3]=m_TargetPoints[3];
	m_TargetPointsTemp[4]=m_TargetPoints[4];
	for(int j=0;j<5;j++)
	{
		if(std::sqrt((0-m_TargetPoints[0].x)*(0-m_TargetPoints[0].x)+(0-m_TargetPoints[0].y)*(0-m_TargetPoints[0].y))>std::sqrt((0-m_TargetPointsTemp[j].x)*(0-m_TargetPointsTemp[j].x)+(0-m_TargetPointsTemp[j].y)*(0-m_TargetPointsTemp[j].y)))
		{
			m_TargetPoints[0]=m_TargetPointsTemp[j];
		}

		if(std::sqrt((800-m_TargetPoints[1].x)*(800-m_TargetPoints[1].x)+(0-m_TargetPoints[1].y)*(0-m_TargetPoints[1].y))>std::sqrt((800-m_TargetPointsTemp[j].x)*(800-m_TargetPointsTemp[j].x)+(0-m_TargetPointsTemp[j].y)*(0-m_TargetPointsTemp[j].y)))
		{
			m_TargetPoints[1]=m_TargetPointsTemp[j];
		}

		if(std::sqrt((800-m_TargetPoints[2].x)*(800-m_TargetPoints[2].x)+(800-m_TargetPoints[2].y)*(800-m_TargetPoints[2].y))>std::sqrt((800-m_TargetPointsTemp[j].x)*(800-m_TargetPointsTemp[j].x)+(800-m_TargetPointsTemp[j].y)*(800-m_TargetPointsTemp[j].y)))
		{
			m_TargetPoints[2]=m_TargetPointsTemp[j];
		}

		if(std::sqrt((0-m_TargetPoints[3].x)*(0-m_TargetPoints[3].x)+(800-m_TargetPoints[3].y)*(800-m_TargetPoints[3].y))>std::sqrt((0-m_TargetPointsTemp[j].x)*(0-m_TargetPointsTemp[j].x)+(800-m_TargetPointsTemp[j].y)*(800-m_TargetPointsTemp[j].y)))
		{
			m_TargetPoints[3]=m_TargetPointsTemp[j];
		}



	}

	for(int j=0;j<5;j++)
	{
		if(m_TargetPoints[3].x!=m_TargetPointsTemp[j].x&&m_TargetPoints[3].y!=m_TargetPointsTemp[j].y&&m_TargetPoints[2].x!=m_TargetPointsTemp[j].x&&m_TargetPoints[2].y!=m_TargetPointsTemp[j].y&&m_TargetPoints[1].x!=m_TargetPointsTemp[j].x&&m_TargetPoints[1].y!=m_TargetPointsTemp[j].y&&m_TargetPoints[0].x!=m_TargetPointsTemp[j].x&&m_TargetPoints[0].y!=m_TargetPointsTemp[j].y)
		{
			m_TargetPoints[4]=m_TargetPointsTemp[j];
		}
	}

	//printf("m_TargetPoints[4].x:%d m_TargetPoints[4].y:%d\n",m_TargetPoints[4].x,m_TargetPoints[4].y);
	//printf("m_TargetPoints[3].x:%d m_TargetPoints[3].y:%d\n",m_TargetPoints[3].x,m_TargetPoints[3].y);
	//printf("m_TargetPoints[2].x:%d m_TargetPoints[2].y:%d\n",m_TargetPoints[2].x,m_TargetPoints[2].y);
	//printf("m_TargetPoints[1].x:%d m_TargetPoints[1].y:%d\n",m_TargetPoints[1].x,m_TargetPoints[1].y);
	//printf("m_TargetPoints[0].x:%d m_TargetPoints[0].y:%d\n",m_TargetPoints[0].x,m_TargetPoints[0].y);


}

void Target::SartUp()
{
	Sort();
	m_iRightTarget=std::abs(m_TargetPoints[1].y-m_TargetPoints[2].y);
	m_iLeftTarget=std::abs(m_TargetPoints[0].y-m_TargetPoints[3].y);
	m_iDownTarget=std::abs(m_TargetPoints[3].x-m_TargetPoints[2].x);
	m_iUpTarget=std::abs(m_TargetPoints[0].x-m_TargetPoints[1].x);
	m_iDiagonal1=std::sqrt(std::abs(m_TargetPoints[1].x-m_TargetPoints[3].x)*std::abs(m_TargetPoints[1].x-m_TargetPoints[3].x)+std::abs(m_TargetPoints[1].y-m_TargetPoints[3].y)*std::abs(m_TargetPoints[1].y-m_TargetPoints[3].y));
	m_iDiagonal2=std::sqrt(std::abs(m_TargetPoints[0].x-m_TargetPoints[2].x)*std::abs(m_TargetPoints[0].x-m_TargetPoints[2].x)+std::abs(m_TargetPoints[0].y-m_TargetPoints[2].y)*std::abs(m_TargetPoints[0].y-m_TargetPoints[2].y));
	m_iRightTargetRef=std::abs(m_TargetPointsRef[1].y-m_TargetPointsRef[2].y);
	m_iLeftTargetRef=std::abs(m_TargetPointsRef[0].y-m_TargetPointsRef[3].y);
	m_iDownTargetRef=std::abs(m_TargetPointsRef[3].x-m_TargetPointsRef[2].x);
	m_iUpTargetRef=std::abs(m_TargetPointsRef[0].x-m_TargetPointsRef[1].x);
	m_TargetCenter.x=m_TargetPoints[0].x+std::abs(m_TargetPoints[0].x-m_TargetPoints[1].x)/2;
	m_TargetCenterRef.x=m_TargetPointsRef[0].x+std::abs(m_TargetPointsRef[0].x-m_TargetPointsRef[1].x)/2;
	m_TargetCenter.y=m_TargetPoints[1].y+std::abs(m_TargetPoints[1].y-m_TargetPoints[2].y)/2;
	m_TargetCenterRef.y=m_TargetPointsRef[1].y+std::abs(m_TargetPointsRef[1].y-m_TargetPointsRef[2].y)/2;
	m_iDiagonal1Ref=std::sqrt(std::abs(m_TargetPointsRef[1].x-m_TargetPointsRef[3].x)*std::abs(m_TargetPointsRef[1].x-m_TargetPointsRef[3].x)+std::abs(m_TargetPointsRef[1].y-m_TargetPointsRef[3].y)*std::abs(m_TargetPointsRef[1].y-m_TargetPointsRef[3].y));
	m_iDiagonal2Ref=std::sqrt(std::abs(m_TargetPointsRef[0].x-m_TargetPointsRef[2].x)*std::abs(m_TargetPointsRef[0].x-m_TargetPointsRef[2].x)+std::abs(m_TargetPointsRef[0].y-m_TargetPointsRef[2].y)*std::abs(m_TargetPointsRef[0].y-m_TargetPointsRef[2].y));
	m_iEstOrientation=m_TargetPoints[4].x-m_TargetCenter.x;
	m_iEstOrientationRef=m_TargetPointsRef[4].x-m_TargetCenterRef.x;
}
//!Protected Methods
//


//!Private Methods
//

/**
 * Starts all the attributes of
 * the Target
 */
void Target::initAttributes ( )
{

}


}
